#ifndef _BSPLINE_H_
#define _BSPLINE_H_

#include "Polynomial.h"
#include "Numtool.h"
#include <set>
#include <algorithm>
#include <fstream>
#include <string>

template<int Dim,int ...Order>
class Bspline;

template<int Order>
class Bspline<1,Order>{
protected:
  std::set<double> knots_;
  std::vector<Polynomial<Order,double> > polys_;
public:
  Bspline(const std::vector<double>& kns);
  Bspline(const std::set<double>& knots = std::set<double>(),
  const std::vector<Polynomial<Order,double> >& polys = std::vector<Polynomial<Order,double> >());
  ~Bspline() = default;
  bool empty() const;
  void setknots(const std::set<double>& knt);
  void setpolys(const std::vector<Polynomial<Order,double> >& polys);
  const std::set<double>& getknots() const;
  std::set<double> getknotsCopy() const;
  const std::vector<Polynomial<Order,double> >& getpolys() const;
  std::vector<Polynomial<Order,double> > getpolysCopy() const;
  void insert(double& d);
  void erase(typename std::vector<Polynomial<Order,double>
  >::const_iterator& it);
  double operator()(const double t) const;
  Bspline<1,Order-1> der() const;
  double rder(const double t, const int m = 1) const;
  void show(std::ostream& os = std::cout) const;
  template<int Ord>
  friend void bsplineplt(const Bspline<1,Ord>& Bs, const std::string
                         &_file, const int num, const bool overwrite);

};

template<int Order>
Bspline<1,Order>::Bspline(const std::vector<double>& kns){
  std::vector<double> knots;
  for (auto it = kns.cbegin();it != kns.cend(); it++){
    knots.push_back(*it);
    knots_.insert(*it);
  }
  std::sort(knots.begin(),knots.end());
  const int numknots = knots.size();
  assert(numknots == Order + 1 && numknots > 1);
  //const int numpiece = knots_.size();
  int k = numknots - 1;
  std::vector<Bspline<1,Order> > vB1(k);
  std::vector<bool> flag(k);
  for (int i = 0 ; i < k ; i++ ){
    if (!equal(knots[i],knots[i+1])){
      std::set<double> tmpset;
      tmpset.insert(knots[i]);
      tmpset.insert(knots[i+1]);
      vB1[i].setknots(tmpset);
      std::vector<Polynomial<Order,double> > tmppoly(1);
      Polynomial<Order,double> p1(1);
      tmppoly[0] = p1;
      vB1[i].setpolys(tmppoly);
    }
    else{
      std::set<double> tmpset;
      tmpset.insert(knots[i]);
      vB1[i].setknots(tmpset);
      flag[i] = true;
    }
  }
  std::vector<std::vector<Bspline<1,Order> > > table(k);
  table[k-1] = vB1;
  for (k-- ; k > 0 ; k--){
    std::vector<Bspline<1,Order> > vB(k);
    for (int i = 0 ; i < k ; i++){
      if (table[k][i].empty() == 0 || table[k][i+1].empty() == 0){
        std::set<double> tmpset = table[k][i].getknotsCopy();
        tmpset.insert(knots[i+numknots-k]);
        vB[i].setknots(tmpset);
        std::vector<Polynomial<Order,double> > tmppoly(numknots-k);
        if (table[k][i].empty()){
          std::vector<Polynomial<Order,double> > vp2 =
          table[k][i+1].getpolysCopy();
          Polynomial<2,double>
      tmp2({-1.0/(knots[i+numknots-k]-knots[i+1]),knots[i+numknots-k]/(knots[i+numknots-k]-knots[i+1])});
          for (int j = 0 ; j < numknots - k - 1; j++)
            vp2[j] = adjust<Order,Order+1>(vp2[j]*tmp2);
          tmppoly[0] = Polynomial<Order,double>(0);
          for (int j = 1 ; j < numknots-k-1; j++)
            tmppoly[j] = vp2[j-1];
          tmppoly[numknots-k-1] = vp2[numknots-k-2];
          vB[i].setpolys(tmppoly);
        }
        else if (table[k][i+1].empty()){
          std::vector<Polynomial<Order,double> > vp1 =
          table[k][i].getpolysCopy();
          Polynomial<2,double>
        tmp1({1.0/(knots[i+numknots-k-1]-knots[i]),-knots[i]/(knots[i+numknots-k-1]-knots[i])});
          for (int j = 0 ; j < numknots - k - 1; j++)
            vp1[j] = adjust<Order,Order+1>(vp1[j]*tmp1);
          tmppoly[0] = vp1[0];
          for (int j = 1 ; j < numknots-k-1; j++)
            tmppoly[j] = vp1[j];
          tmppoly[numknots-k-1] = Polynomial<Order,double>(0);
          vB[i].setpolys(tmppoly);
        }
        else{
          std::vector<Polynomial<Order,double> > vp1 = table[k][i].getpolysCopy();
          std::vector<Polynomial<Order,double> > vp2 = table[k][i+1].getpolysCopy();
          Polynomial<2,double>
        tmp1({1.0/(knots[i+numknots-k-1]-knots[i]),-knots[i]/(knots[i+numknots-k-1]-knots[i])});
          Polynomial<2,double>
      tmp2({-1.0/(knots[i+numknots-k]-knots[i+1]),knots[i+numknots-k]/(knots[i+numknots-k]-knots[i+1])});
          for (int j = 0 ; j < numknots - k - 1; j++){
            vp1[j] = adjust<Order,Order+1>(vp1[j]*tmp1);
            vp2[j] = adjust<Order,Order+1>(vp2[j]*tmp2);
          }
          tmppoly[0] = vp1[0];
          for (int j = 1 ; j < numknots-k-1; j++)
            tmppoly[j] = vp1[j] + vp2[j-1];
          tmppoly[numknots-k-1] = vp2[numknots-k-2];
          vB[i].setpolys(tmppoly);
        }
      
      }
      
    }
    table[k-1] = vB;
  }
  const std::vector<Polynomial<Order,double> >& respoly =
    table[0][0].getpolys();
  this->setpolys(respoly);
  auto it = polys_.begin();
  for (int i = 0 ; i < numknots - 1 ; i++ ){
    if (flag[i]){
      polys_.erase(it);
    }
    else
      it++;
  }
}

template<int Order>
bool Bspline<1,Order>::empty() const{
  return polys_.empty();
}

template<int Order>
inline Bspline<1,Order>::Bspline(const std::set<double>& knots, const std::vector<Polynomial<Order,double> >& polys):knots_(knots),polys_(polys){}

template<int Order>
inline void Bspline<1,Order>::setknots(const std::set<double>& knt){
  knots_ = knt;
}

template<int Order>
inline void Bspline<1,Order>::setpolys(const std::vector<Polynomial<Order,double> >& polys){
  polys_ = polys;
}

template<int Order>
inline const std::set<double>& Bspline<1,Order>::getknots() const{
  return knots_;
}

template<int Order>
inline const std::vector<Polynomial<Order,double> >& Bspline<1,Order>::getpolys() const{
  return polys_;
}

template<int Order>
inline std::set<double> Bspline<1,Order>::getknotsCopy() const{
  return knots_;
}

template<int Order>
inline std::vector<Polynomial<Order,double> > Bspline<1,Order>::getpolysCopy() const{
  return polys_;
}

template<int Order>
inline void Bspline<1,Order>::insert(double& d){
  knots_.insert(d);
}

template<int Order>
inline void Bspline<1,Order>::erase(typename std::vector<Polynomial<Order,double> >::const_iterator& it){
  polys_.erase(it);
}

template<int Order>
double Bspline<1,Order>::operator()(const double t) const{
  return rder(t,0);
}

template<int Order>
Bspline<1,Order-1> Bspline<1,Order>::der() const{
  Bspline<1,Order-1> res;
  res.setknots(knots_);
  std::vector<Polynomial<Order-1,double> > newpolys;
  for (auto p:polys_)
    newpolys.push_back(p.der());
  res.setpolys(newpolys);
  return res;
}

template<int Order>
double Bspline<1,Order>::rder(const double t, const int m) const{
  constexpr double tol = 1e-16;
  assert(m >= 0);
  if (m >= Order)
    return 0;
  auto it1 = knots_.upper_bound(t+tol);
  auto it2 = knots_.lower_bound(t-tol);
  if (it1 == knots_.begin())
    return 0;
  else if (it2 == knots_.end())
    return 0;
  it1--;
  int num = std::distance(knots_.begin(),it1);
  if (equal(t,*(--knots_.end())))
    num--;
  auto res = polys_[num];
  int i = m;
  while( i-- > 0)
    res = adjust<Order,Order-1>(res.der());
  return res(t);
}

template<int Order>
void Bspline<1,Order>::show(std::ostream& os) const{
  auto it = knots_.begin();
  for (int i = 0 ; i < (int)polys_.size() ; i++)
    os << "[" << *it << "," << *(++it) << "]: " << polys_[i] << std::endl;
}

template<int Ord>
void bsplineplt(const Bspline<1,Ord>& Bs, const std::string
                &_file, const int num = 200, const bool overwrite = 0){
  const std::vector<Polynomial<Ord,double> >& polys = Bs.getpolys();
  assert(!polys.empty());
  const std::set<double>& knots = Bs.getknots();
  std::vector<double> spknots(knots.begin(),knots.end());
  std::ofstream os;
  if (overwrite)
    os.open(_file,std::ios::app);
  else
    os.open(_file);
  double h = (spknots.back() - spknots.front())/num;
  std::vector<double> y(num+1);
  int j = 0;
  double t = spknots.front();
  os << "t = [";
  for (int i = 0; i < num ; i++){
    if (t >= spknots[j+1])
      j++;
    y[i] = (polys[j])(t);
    os << t << ",";
    t += h;
  }
  os << t;
  y[num] = (polys[j])(t);
  os << "];\n" << "y = [";
  for (int i = 0; i <= num ; i++)
    os << y[i] << ",";
  os << "];\n" << "plot(t,y);";
  os.close();
}

#endif // _BSPLINE_H_
